% 恒速优化，版本二
function state = speedplan_cruise2(lineStruct, trainStruct, qsStruct)
%% 求解参数初始化
% 待求解问题
set_time = qsStruct.set_time; %剩余运行时间 s
pos_start = qsStruct.pos_start * 1000; %起始位置 m
pos_end = qsStruct.pos_end * 1000; %末位置 m
v_start = qsStruct.v_start; %初始速度 km/h
v_end = qsStruct.v_end; %末速度 km/h
step_x = qsStruct.step_x; %求解步长
% 线
Speed_Limit = lineStruct.speedlimit;
Gradient = lineStruct.gradient;
Curve = lineStruct.curve;
% 车
abc = trainStruct.abc;
train_mass = trainStruct.mass;
%% 问题初始化
x_ = [pos_start:step_x:pos_end]'; %位置离散
sum_num_state = length(x_);
%数据结构：[位置/m，速度/km/h，时刻/s，牵引力/kN，制动力/kN，基本阻力/kN，加速度/m/s^2，时间间隔/s，工况状态，限速/km/h，单位坡度阻力/千分号，单位曲线阻力/s，海拔/m]
State = zeros(sum_num_state,13);
State(:,1) = x_;
%对每个位置的限速曲线进行赋值
for i = 1:sum_num_state
    State(i,10) = inquire_linedata(State(i,1), Speed_Limit);
end
%对每个位置的单位坡度阻力进行赋值
for i = 1:sum_num_state
    State(i,11) = inquire_linedata(State(i,1), Gradient);
end
%对每个位置的单位曲线阻力进行赋值
for i = 1:sum_num_state
    crv = inquire_linedata(State(i,1), Curve);
    if crv ~= 0
        State(i,12) = 600 / abs(crv);
    else
        State(i,12) = 0;
    end
end
%对每个位置的海拔进行赋值
for i = 1:sum_num_state
    State(i,13) = getPosHeight(State(i,1),lineStruct);
end

%% 先算一遍最大能力，得最短运行时间
state_max = speedPlanMP(lineStruct,trainStruct,qsStruct);
t_min = state_max(end,3);
if set_time < t_min
    fprintf('时分约束小于最大能力！');
    state = state_max;
    return;
end

%% 恒速优化
%先反算制动段
last_brake_state(1,:) = state(end,:);
last_brake_state(1,5) = getMaxFb(0);
i = 1;
while 1
    last_brake_state(i+1,:) = state(end-i,:);
    %位置差
    dx_m = last_brake_state(i+1,1) - last_brake_state(i,1);
    %i的速度
    vi_kmh = last_brake_state(i,1);
    %i的阻力
    f_res = (cal_unit_basicResist(abc, vi_kmh) + last_brake_state(i,11) + last_brake_state(i,12)) * train_mass*9.81/1000;
    %
end



